#pragma once
#include "g2o/core/base_unary_edge.h"
#include "graph_optimizer/vertex/vertex_p.hpp"

namespace g2o
{
  class EdgeGnss : public BaseUnaryEdge<3, Eigen::Vector3d, VertexP>
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgeGnss();
    virtual bool read(std::istream &is);
    virtual bool write(std::ostream &os) const;

    void computeError();

    // jacobian
    virtual void linearizeOplus();

    virtual void setMeasurement(const Eigen::Vector3d &m)
    {
      _measurement = m;
    }

    virtual bool setMeasurementData(const double *d)
    {
      Eigen::Map<const Eigen::Vector3d> v(d);
      _measurement = v;
      return true;
    }

    virtual bool getMeasurementData(double *d) const
    {
      Eigen::Map<Eigen::Vector3d> v(d);
      v = _measurement;
      return true;
    }

    virtual int measurementDimension() const { return 3; }

    virtual bool setMeasurementFromState();

    virtual double initialEstimatePossible(const OptimizableGraph::VertexSet & /*from*/,
                                           OptimizableGraph::Vertex * /*to*/)
    {
      return 0;
    }
  };

} // namespace g2o